/**
 ******************************************************************************
 * @file           : PID.c
 * @author         : yall
 * @brief          : PID控制
 * @attention      : None
 * @date           : 2025/6/25
 ******************************************************************************
 */
#include "PID.h"

// 用于初始化pid参数的函数
void PID_Init(PID *pid, float p, float i, float d, float maxI, float maxOut)
{
    pid->kp = p;
    pid->ki = i;
    pid->kd = d;
    pid->maxIntegral = maxI;
    pid->maxOutput = maxOut;
}
 
// 计算过程
void PID_Calc(PID *pid, float set, float now)
{
 	//数据
    pid->lastError = pid->error; 
    pid->error = set - now; 

    //微分
    float dout = (pid->error - pid->lastError) * pid->kd;

    //比例
    float pout = pid->error * pid->kp;

    //积分
    pid->integral += pid->error * pid->ki;

    //限幅
    if(pid->integral > pid->maxIntegral) pid->integral = pid->maxIntegral;
    else if(pid->integral < -pid->maxIntegral) pid->integral = -pid->maxIntegral;

    //输出
    pid->output = pout + dout + pid->integral;

    //输出限幅
    if(pid->output > pid->maxOutput) pid->output =   pid->maxOutput;
    else if(pid->output < -pid->maxOutput) pid->output = -pid->maxOutput;
}
 
 // 线性映射
float linear_map(float x, float A, float B, float C, float D) 
{
    if (B == A) {  // 防止除以零
        return (C + D) / 2.0f;
    }
    return C + (x - A) * (D - C) / (B - A);
}

 